package a_star

import "math"

type tAStarPathFinder struct {
}

func newAStarPathFinder() IPathFinder {
	return &tAStarPathFinder{}
}

func (me *tAStarPathFinder) FindPath(nodes []INode, lines []ILine, srcID string, dstID string) (bool,[]INode) {
	// 节点索引
	mapNodes := make(map[string]INode, 0)
	for _,it := range nodes {
		mapNodes[it.ID()] = it
	}

	srcNode, ok := mapNodes[srcID]
	if !ok {
		return false, nil
	}

	dstNode,ok := mapNodes[dstID]
	if !ok {
		return false, nil
	}

	// 边的索引
	mapFromLines := make(map[string][]ILine, 0)
	mapToLines := make(map[string][]ILine, 0)
	for _, it := range lines {
		if v,ok := mapFromLines[it.From()];ok {
			mapFromLines[it.From()] = append(v, it)
		} else {
			mapFromLines[it.From()] = []ILine{ it }
		}

		if v,ok := mapToLines[it.To()];ok {
			mapToLines[it.To()] = append(v, it)
		} else {
			mapToLines[it.To()] = []ILine{ it }
		}
	}


	for _,it := range nodes {
		// 设置src节点的weight为0, 其他节点的weight为MaxWeight
		if it.ID() == srcID {
			it.SetMeasuredWeight(0)
		} else {
			it.SetMeasuredWeight(MaxWeight)
		}

		// 计算每个节点到dst节点的估算距离
		if it.ID() == dstID {
			it.SetEstimatedWeight(0)
		} else {
			it.SetEstimatedWeight(me.distance(it.GetX(), it.GetY(), dstNode.GetY(), dstNode.GetY()))
		}
	}


	// 将起点push到堆
	heap := newArrayHeap(newNodeWeightComparator())
	heap.Push(srcNode)

	// 遍历候选节点
	for heap.IsNotEmpty() {
		_, top := heap.Pop()
		from := top.(INode)
		if from.ID() == dstID {
			break
		}

		links, ok := mapFromLines[from.ID()]
		if ok {
			for _,line := range links {
				if to,ok := mapNodes[line.To()];ok {
					if me.updateMeasuredWeight(from, to, line) {
						n := heap.IndexOf(to)
						if n >= 0 {
							heap.ShiftUp(n)
						} else {
							heap.Push(to)
						}
					}
				}
			}
		}
	}

	// 逆向查找最短路径
	if dstNode.GetMeasuredWeight() >= MaxWeight {
		return false, nil
	}

	path := []INode{ dstNode }
	current := dstNode
	maxRound := len(lines)
	for ;current != srcNode && maxRound > 0;maxRound-- {
		linkedLines, _ := mapToLines[current.ID()]
		for _,line := range linkedLines {
			from, _ := mapNodes[line.From()]
			if from.GetMeasuredWeight() == current.GetMeasuredWeight() - line.Weight() {
				current = from
				path = append(path, from)
			}
		}
	}

	if current != srcNode {
		return false, nil
	}

	me.reverse(path)
	return true, path
}

func (me *tAStarPathFinder) distance(x0, y0, x1, y1 int) int {
	dx := x0 - x1
	dy := y0 - y1
	return int(math.Round(math.Sqrt(float64(dx * dx + dy * dy))))
}


func (me *tAStarPathFinder) reverse(nodes []INode) {
	for i,j := 0, len(nodes)-1;i < j;i,j=i+1,j-1 {
		nodes[i], nodes[j] = nodes[j], nodes[i]
	}
}

func (me *tAStarPathFinder) updateMeasuredWeight(from INode, to INode, line ILine) bool {
	w := me.min(from.GetMeasuredWeight() + line.Weight(), to.GetMeasuredWeight())
	if to.GetMeasuredWeight() > w {
		to.SetMeasuredWeight(w)
		return true
	}

	return false
}


func (me *tAStarPathFinder) min(a, b int) int {
	if a <= b {
		return a
	}
	return b
}


var AStarPathFinder = newAStarPathFinder()